#include <types.h>
#include <Debug.h>
#include <CPU.h>
#include <Paging.h>

#include <stdlib.h>
#include <string.h>

#define RELOAD_PGDIR()	CPU::WriteCR3(CPU::ReadCR3())

PageDirectory PgDir __attribute__ ((aligned(0x1000)));
PageTable FirstHeapTable __attribute__ ((aligned(0x1000)));

void Paging::Init() {
	for (int i = 0; i < 768; i++) {
		PgDir.Tables[i] = (PageTable *) ((i << 22) | 0x83);
		PgDir.TablesPhys[i] = (i << 22) | 0x83;
	}
	
	PgDir.Tables[832] = &FirstHeapTable;
	PgDir.TablesPhys[832] = (dword) &FirstHeapTable | 3;

	CPU::WriteCR3((dword) &PgDir.TablesPhys);
	
	dword tmp = CPU::ReadCR4();
	tmp |= 0x10;
	CPU::WriteCR4(tmp);

	tmp = CPU::ReadCR0();
	tmp |= 0xC0010000;
	CPU::WriteCR0(tmp);
}

void Paging::MapMemory(word flags, dword virt, dword phys, dword size) {
	while (size) {
		MapPage(flags, virt, phys);
		virt += 0x1000;
		phys += 0x1000;
		size -= 0x1000;
	}
	RELOAD_PGDIR();
}

void Paging::MapPage(word flags, dword virt, dword phys) {
	PageTable *tbl = PgDir.Tables[PGDIR_INDEX(virt)];
	if (!tbl) {
		PgDir.Tables[PGDIR_INDEX(virt)] = (PageTable *) valloc(sizeof(PageTable));
		tbl = PgDir.Tables[PGDIR_INDEX(virt)];
		memset(tbl, 0, sizeof(PageTable));
		PgDir.TablesPhys[PGDIR_INDEX(virt)] = virt2phys((dword) tbl) | 7;
	}

	/* Set page entry */
	tbl->Pages[PGTAB_INDEX(virt)].frame = phys >> 12;
	tbl->Pages[PGTAB_INDEX(virt)].present = 1;
	tbl->Pages[PGTAB_INDEX(virt)].rw = (flags & PAGE_FLAGS_WRITE) ? 1 : 0;
	
	RELOAD_PGDIR();
}

dword Paging::virt2phys(dword virt) {
	PageTable *tbl = PgDir.Tables[PGDIR_INDEX(virt)];
	if (!tbl) {
		return (dword) NULL;
	}
	return tbl->Pages[PGTAB_INDEX(virt)].frame << 12;
}

